#include "pixeltransform.h"
#include "blobDetection.h"
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <opencv-2.3.1/opencv2/highgui/highgui.hpp>

// GUI STUFF
string windowname;
int objectID = -1;
int colorHue = 0;
int colorSat = 128;
int colorSpan = 10;

#include <irb_msgs/ObjectPose.h>

using namespace std;

BlobDetection *blobDetector;

ros::Publisher image_pub_detected;
ros::Publisher posePub;


/******************************************************************
Callback function for drawing the system interface to the window
****************************************************************/
void drawCallback(const ros::TimerEvent&)
{
	cv::Mat image(50, 500, CV_8UC3);
	image = cv::Scalar(colorHue,colorSat,255);
	cv::cvtColor(image, image, CV_HSV2BGR);

	cv::imshow(windowname, image);
	cv::waitKey(1);
}

/*************************************************************************************
 Function for fetching new images from the camera input and convert them to
 openCV types and use the blobDetection class to detect blobs in the fetched image.
*************************************************************************************/
void imageCallback(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &camInfo)
{
	cv_bridge::CvImagePtr cv_ptr;

	/**************************************************************************
  * Convert image to openCV format using the cv_bridge function.
  * If the operation fails give an error message.
  **************************************************************************/
	try
	{
		cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
	}
	catch(cv_bridge::Exception& e)
	{
		ROS_ERROR("Something went wrong when converting!");
		return;
	}

	// Set color hue/span from GUI
	blobDetector->setHuecolorSpan(colorHue, colorSpan, colorSat);

	// Detect blobs
	blobDetector->updateModel(camInfo);
	blobDetector->detectBlobs(cv_ptr->image);

	vector<detectedObject> objects = blobDetector->GetObjects();
	PixelTransform& pixelTransform = blobDetector->GetTransform();

	static geometry_msgs::Point oldPoint;

	ROS_INFO("Found: %d objects", objects.size());

	for(vector<detectedObject>::iterator it = objects.begin(); it != objects.end(); it++)
	{
		detectedObject obj = *it;

		geometry_msgs::Point basePoint;
		bool success = pixelTransform.transformFrom3dToBase(obj.mObjectPos * 1000, basePoint); // NOTE: 1000 to transform from m to mm

		if(!success)
		{
			ROS_ERROR("Failed to transform from camera to base!");
			continue;
		}

		basePoint.x = (7.0* oldPoint.x + basePoint.x + 4.0) / 8.0;
		basePoint.y = (7.0* oldPoint.y + basePoint.y + 4.0) / 8.0;
		basePoint.z = (7.0* oldPoint.z + basePoint.z + 4.0) / 8.0;

		oldPoint = basePoint;

		ROS_INFO("Found: X: %f Y: %f Z: %f", basePoint.x, basePoint.y, basePoint.z);

		// How to use the msg that contains each blob position and orientation relative to the base of the robot
		irb_msgs::ObjectPose pose;
		pose.ID = objectID;
		pose.pose.position = basePoint;
		pose.pose.orientation.x = 0;
                pose.pose.orientation.y = sqrt(1/2.0);
		pose.pose.orientation.z = 0;
                pose.pose.orientation.w = sqrt(1/2.0);

		cv::circle(cv_ptr->image, obj.screenPos, 4, cv::Scalar(255, 0, 255));
		posePub.publish(pose);
	}

	/**************************************************
  * Publish debug images
  **************************************************/

	image_pub_detected.publish(cv_ptr->toImageMsg());
}


int main(int argc, char ** argv)
{
	/**
 * The ros::init() function needs to see argc and argv so that it can perform
 * any ROS arguments and name remapping that were provided at the command line. For programmatic
 * remappings you can use a different version of init() which takes remappings
 * directly, but for most command-line programs, passing argc and argv is the easiest
 * way to do it.  The third argument to init() is the name of the node.
 *
 * You must call one of the versions of ros::init() before using any other
 * part of the ROS system.
 */
	ros::init(argc, argv, "simpleDetector");

	if(argc != 2)
	{
		ROS_ERROR("Missing or too many arguments, usage: %s <objectID>", argv[0]);
		return 1;
	}


	// Handle args
	for(int i = 1; i < argc; i++)
	{
		string arg(argv[i]);
		stringstream ss(arg);
		ss >> objectID;
		windowname = "SimpleTracker, ID: " + arg;
	}

	// Set up GUI
	cv::namedWindow(windowname);
	cv::createTrackbar("Color: Hue", windowname, &colorHue, 180);
	cv::createTrackbar("Color: Span", windowname, &colorSpan, 100);
	cv::createTrackbar("Color: Sat", windowname, &colorSat, 255);

	/**
 * NodeHandle is the main access point to communications with the ROS system.
 * The first NodeHandle constructed will fully initialize this node, and the last
 * NodeHandle destructed will close down the node.
 */
	ros::NodeHandle n;

	blobDetector = new BlobDetection(n);

	/**
 * The subscribe() call is how you tell ROS that you want to receive messages
 * on a given topic.  This invokes a call to the ROS
 * master node, which keeps a registry of who is publishing and who
 * is subscribing.  Messages are passed to a callback function, here
 * called chatterCallback.  subscribe() returns a Subscriber object that you
 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
 * object go out of scope, this callback will automatically be unsubscribed from
 * this topic.
 *
 * The second parameter to the subscribe() function is the size of the message
 * queue.  If messages are arriving faster than they are being processed, this
 * is the number of messages that will be buffered up before beginning to throw
 * away the oldest ones.
 */
	image_transport::ImageTransport imageTransport = image_transport::ImageTransport(n);
	image_transport::CameraSubscriber camMagic = imageTransport.subscribeCamera("camera/rgb/image_rect_color", 1, imageCallback);


	image_pub_detected = n.advertise<sensor_msgs::Image>("irb_picker/tracker/detectedBlobs", 1);
	posePub = n.advertise<irb_msgs::ObjectPose>("pose", 1000);

	// Create a periodic callback that will perform drawing (to display information previously gathered)
	ros::Timer timer = n.createTimer(ros::Duration(0.1), drawCallback);

	/**
 * ros::spin() will enter a loop, pumping callbacks.  With this version, all
 * callbacks will be called from within this thread (the main one).  ros::spin()
 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
 */
	ros::spin();

	return 0;
}
